#include <dh_hand_driver/ag95_grasp.h>
// client
#include<stdio.h>
#include<stdlib.h>
#include<string.h>
#include<errno.h>
#include<sys/types.h>
#include<sys/socket.h>
#include<netinet/in.h>
#include<arpa/inet.h>
#include<unistd.h>
#include<thread>
#include<mutex>
#define MAXLINE 4096
// client
using namespace std;

mutex m;
int FLAG = 0;

int client() {
    // cout << "shit\n";
    // int   sockfd, n;
    // char  recvline[4096], sendline[4096];
    // struct sockaddr_in  servaddr;

    // if( (sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0){
    //     printf("create socket error: %s(errno: %d)\n", strerror(errno),errno);
    //     return 0;
    // }

    // memset(&servaddr, 0, sizeof(servaddr));
    // servaddr.sin_family = AF_INET;
    // servaddr.sin_port = htons(6666);
    // string ip = "127.0.0.1";
    // if( inet_pton(AF_INET, ip.c_str(), &servaddr.sin_addr) <= 0){
    //     printf("inet_pton error for %s\n", ip.c_str());
    //     return 0;
    // }

    // if( connect(sockfd, (struct sockaddr*)&servaddr, sizeof(servaddr)) < 0){
    //     printf("connect error: %s(errno: %d)\n",strerror(errno),errno);
    //     return 0;
    // }

    // printf("send msg to server: \n");
    // fgets(sendline, 4096, stdin);
    // if( send(sockfd, sendline, strlen(sendline), 0) < 0){
    //     printf("send msg error: %s(errno: %d)\n", strerror(errno), errno);
    //     return 0;
    // }
    // close(sockfd);
    // return 0;
    m.lock();

    int socket_fd = socket(AF_INET, SOCK_STREAM, 0);
    if (socket_fd == -1)
    {
        cout << "socket 创建失败： "<< endl;
        exit(1);
    }
    
    struct sockaddr_in addr;
    addr.sin_family = AF_INET;
    addr.sin_port = htons(8888);
    addr.sin_addr.s_addr = inet_addr("49.123.88.1");

    int res = bind(socket_fd,(struct sockaddr*)&addr,sizeof(addr));
    if (res == -1)
    {
        cout << "bind创建失败： " << endl;
        exit(-1);
    }
    cout << "bind ok 等待客户端的连接" << endl;

    listen(socket_fd, 5);

    struct sockaddr_in client;
    socklen_t len = sizeof(client);
    int fd = accept(socket_fd,(struct sockaddr*)&client,&len);
    if (fd == -1)
    {
        cout << "accept错误\n" << endl;
        exit(-1);
    }

    char *ip = inet_ntoa(client.sin_addr);
    cout << "客户： 【" << ip << "】连接成功" << endl;
   
    char buffer[255]={};
    int size = read(fd, buffer, sizeof(buffer));//通过fd与客户端联系在一起,返回接收到的字节数

    cout << "接收到字节数为： " << size << endl;
    cout << "内容： " << (buffer[0] == '1'? 1 : 0) << endl;
    if (buffer[0] == '1') FLAG = 1;
    if (buffer[0] == '2') FLAG = 2;
    close(fd);
    close(socket_fd);
    m.unlock();
    return 0;
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "test_hand");
  ros::NodeHandle n;
  thread t(client);
  t.detach();
  // AG95_grasp(n, 1, 10, 20);
  // AG95_grasp(n, 1, 80, 20);
  // AG95_grasp(n, 1, 10, 20);
  // AG95_grasp(n, 1, 80, 20);
  // AG95_grasp(n, 1, 10, 20);
  // AG95_grasp(n, 1, 80, 20);
  // AG95_grasp(n, 1, 10, 20);
  // AG95_grasp(n, 1, 80, 20);
  sleep(1);
  m.lock();
  if (FLAG == 1) AG95_grasp(n, 1, 20, 25);
  if (FLAG == 2) AG95_grasp(n, 1, 80, 25);
  m.unlock();
  ros::shutdown();
  return 0;
}